#ifndef I_LIB_VISIONAD_I_GROUND_H
#define I_LIB_VISIONAD_I_GROUND_H

#include <vector>
#include "i_cammodel.h"

namespace idl
{
    struct ADGroundPlane
    {
        ADGroundPlane() {
            i_zero4(params);
            nr_support = 0;
        }

        ADGroundPlane& operator=(const ADGroundPlane& pi) {
            i_copy4(pi.params, this->params);
            this->nr_support = pi.get_nr_support(); 
            return(*this);
        }

        float get_degree_normal_to_x() const {
            float normal[3];
            i_copy3(params, normal);
            if (normal[0] < 0) {
                i_neg3(normal);
            }
            //normalize:
            i_scale3(normal, i_rec(i_sqrt(i_squaresum3(normal))));
            return i_radians_to_degree(i_acos(normal[0]));
        }

        float get_degree_normal_to_y() const {
            float normal[3];
            i_copy3(params, normal);
            if (normal[1] < 0) {
                i_neg3(normal);
            }
            //normalize:
            i_scale3(normal, i_rec(i_sqrt(i_squaresum3(normal))));
            return i_radians_to_degree(i_acos(normal[1]));
        }

        float get_degree_normal_to_z() const {
            float normal[3];
            i_copy3(params, normal);
            if (normal[2] < 0) {
                i_neg3(normal);
            }
            //normalize:
            i_scale3(normal, i_rec(i_sqrt(i_squaresum3(normal))));
            return i_radians_to_degree(i_acos(normal[2]));
        }

        void force_positive_normal_x() {
            if (params[0] < 0) {
                i_neg4(params);
            }
        }

        void force_positive_normal_y() {
            if (params[1] < 0) {
                i_neg4(params);
            }
        }

        void force_positive_normal_z() {
            if (params[2] < 0) {
                i_neg4(params);
            }
        }

        void force_invalid() {
            i_zero4(params);
            nr_support = 0;
        }

        void force_unit_norm() {
            float norm = i_l2_norm3(params);
            i_scale4(params, i_rec(norm));
        }

        bool is_valid() const {
            if (nr_support &&
                (params[0] != 0 ||
                 params[1] != 0 ||
                 params[2] != 0)) {
                return true;
            } else {
                return false;
            }
        }

        int get_nr_support() const {
            return nr_support;
        }

        void set_nr_support(int nr) {
            nr_support = nr;
        }

        float params[4];
    private:
        int nr_support;
    };

    struct ADGroundPlaneSpherical
    {
        ADGroundPlaneSpherical() {
            theta = phi = d = 0;
            nr_support = 0;
        }

        ADGroundPlaneSpherical& operator=(const ADGroundPlaneSpherical& pi) {
            this->theta = pi.theta;
            this->phi = pi.phi;
            this->d = pi.d;
            this->nr_support = pi.get_nr_support();
            return (*this);
        }

        bool is_valid() const {
            return (nr_support > 0);
        }

        void force_invalid() {
            theta = phi = d = 0;
            nr_support = 0;
        }

        int get_nr_support() const {
            return nr_support;
        }

        void set_nr_support(int nr) {
            nr_support = nr;
        }

        float theta;
        float phi;
        float d;
    private:
        int nr_support;
    };

    class ADGroundTrackManager {
    public:
        explicit ADGroundTrackManager(int track_length);
        ~ADGroundTrackManager();
        void push(const ADGroundPlane& plane);
        void get_ground(float params[4]);
        void restart();
    private:
        int _head;
        std::vector<ADGroundPlaneSpherical> _tracks;
        std::vector<float> _const_weight_temporal;
        std::vector<float> _weight;
        void plane_eucli_to_spher(const ADGroundPlane& src, ADGroundPlaneSpherical& dst);
        void plane_spher_to_eucli(const ADGroundPlaneSpherical& src, ADGroundPlane& dst);
        void plane_spher_to_eucli(const ADGroundPlaneSpherical& src, float params[4]);
    };

    struct ADBinocularGroundParam
    {
        ADBinocularGroundParam() {
            set_default();
        }
        void set_default();
        float skyline;
        int thre_gpf_inliers;
        int thre_gpf_sample_min;
        int thre_gpf_sample_max;
        float thre_gpf_dist;
        int thre_gpf_ransac_iter;
        float thre_gpf_z_max;
        int gpf_roi_ignored_width;
        float thre_gpf_tilt_angle;
    };

    class ADBinocularGroundTracker
    {
    public:
        ADBinocularGroundTracker(int width, int height, int track_length,
            const ADCameraIntrinsic<float>& intrinsic);
        ~ADBinocularGroundTracker();
        int track(const unsigned char *const *disp,
            const std::vector<float>& depthlut,
            float params[4]);
        void restart(); //firewall - avoid error propagation
        //void visualize(unsigned char *canvas, const float plane[4], int width, int height);
    private:
        int _width;
        int _height;
        int _rand_seed;
        ADBinocularGroundParam _param;
        float *_threeds;
        int **_indices;
        std::vector<ADGroundPlane> _hypothesis;
        ADCameraIntrinsic<float> _camera_intrinsic;
        ADCameraExtrinsic<float> _camera_extrinsic;
        ADGroundTrackManager* _tracker;
        int detect(const unsigned char *const *disp, 
            const std::vector<float>& depthlut,
            ADGroundPlane& plane,
            const int roi[4], int width, int height);
    };
} //namespace idl

#endif